#include "LGH_Cascade_Control.hpp"


MotorValue_S
LGH_Motor1,//���1���Ӻ��濴���Ͻǣ���ʱ����
LGH_Motor2,
LGH_Motor3,
LGH_Motor4;

short int Car_SpeedX=0;
short int Car_SpeedY=0;
short int Car_SpeedZ=0;
short int AIM_SpeedX=0;//放狗用

long int Car_DistanceX=0;
double Car_AngleZ=0;

unsigned char angle_sem_releas_flag=0;//��ֵΪ0
unsigned char aim_sem_releas_flag=0;//��ֵΪ0

long int Car_Encoder_SumX;

short int Car_MaxSpeedX=450;
short int Car_MaxSpeedY=200;
short int Car_MaxSpeedZ=250;

long int Car_MaxDuty=35000;
char Xstatus=0;

short int speed_up_cnt_X=0;

short int startmaxspeed_X=0;

short int increase_rat_X=4;



char Main_Switch=Stop_Switch;//�������⻷����




float Aim_Real_Xcoord,Aim_Real_Ycoord,Aim_Set_Xcoord,Aim_Set_Ycoord;


void Set_Move_X(double DistanceX)
{
	long int dis;
	if(DistanceX>0&&DistanceX<=100)
	{
		dis=(long int)DistanceX*549.00f;
	}
	else if(DistanceX>100)//&&DistanceX<=200
	{
		dis=(long int)(DistanceX*502.00f+(DistanceX-100)*0.51f);
	}
	else if(DistanceX<0&&DistanceX>=-100)
	{
		dis=(long int)DistanceX*549.00f;
	}
	else if(DistanceX<-100)//&&DistanceX<=200
	{
		dis=(long int)(DistanceX*502.00f+(DistanceX-100)*0.51f);
	}
	Car_DistanceX=dis;
	Main_Switch=X_Cascade_Switch;
}


void Set_Aim_XY(double Xcoord,double Ycoord)
{
	Aim_Set_Xcoord=Xcoord;
	Aim_Set_Ycoord=Ycoord;
  Main_Switch=AIM_Switch;
}

void Set_RealAim_XY(double Xcoord,double Ycoord)
{
	Aim_Real_Xcoord=Xcoord;
	Aim_Real_Ycoord=Ycoord;
}


void Move_Rest(void)
{
	Car_SpeedX=0;
	Car_SpeedY=0;
	Car_SpeedZ=0;
	Xstatus=0;   
	Car_Encoder_SumX=0;
	Car_DistanceX=0;
}


void Close_Move_X(void)
{
	Move_Rest();
	Main_Switch=Stop_Switch;
}

void Close_Aim_XY(void)
{
	Move_Rest();
	Main_Switch=Stop_Switch;
}



void Set_Motor_Pwm(void)
{

	switch (Main_Switch)
	{
		case Stop_Switch:
		{
			Car_SpeedX=0;
			Car_SpeedY=0;
			Car_SpeedZ=0;
			Xstatus=0; 
			Car_Encoder_SumX=0;
			Car_DistanceX=0;
			break;
		}
		case X_Cascade_Switch:
		{

			if(Xstatus==0)
			{
			Car_SpeedX=PositPID(&X_P_PID,Car_Encoder_SumX,Car_DistanceX);
			if(Car_DistanceX!=0)
				{
					Xstatus=1;
				}
			}
			if(Xstatus==1)//提速阶段
			{
				if(Car_DistanceX>0)
				{
					Car_SpeedX=PositPID(&X_P_PID,Car_Encoder_SumX,Car_DistanceX);
					if(Car_SpeedX<=startmaxspeed_X)
					{
						Xstatus=2;
					}
					else if(Car_SpeedX>=startmaxspeed_X+speed_up_cnt_X)
					{
						speed_up_cnt_X+=increase_rat_X;
						Car_SpeedX=startmaxspeed_X+speed_up_cnt_X;
					}
					else
					{
						Xstatus=2;
						speed_up_cnt_X=0;
					}
				}
				if(Car_DistanceX<0)
				{
					Car_SpeedX=PositPID(&X_P_PID,Car_Encoder_SumX,Car_DistanceX);
					if(Car_SpeedX>=-startmaxspeed_X)
					{
						Xstatus=2;
					}
					else if(Car_SpeedX<=-startmaxspeed_X-speed_up_cnt_X)
					{
						speed_up_cnt_X+=increase_rat_X;
						Car_SpeedX=-startmaxspeed_X-speed_up_cnt_X;
					}
					else
					{
						Xstatus=2;
						speed_up_cnt_X=0;
					}
				}
			}
			if(Xstatus==2)//位置闭环阶段
			{
				Car_SpeedX=PositPID(&X_P_PID,Car_Encoder_SumX,Car_DistanceX);	
				// AIM_SpeedX=-PositPID(&AIM_Y_PID,Aim_Real_Ycoord*100,Aim_Set_Ycoord*100);//计算jia'z
				if(abs(Car_Encoder_SumX-Car_DistanceX)<40||abs(Car_SpeedX)<30)
				{
					Xstatus=0;
					Car_Encoder_SumX=0;
					Car_DistanceX=0;
					Car_SpeedX=0;
					buzzflag=4;
					rt_sem_release(movx_sem);
				}
			}
			break;
		}
		case AIM_Switch:
		{
			Car_SpeedY=PositPID(&AIM_X_PID,Aim_Real_Xcoord*100,Aim_Set_Xcoord*100);
			Car_SpeedX=-PositPID(&AIM_Y_PID,Aim_Real_Ycoord*100,Aim_Set_Ycoord*100);
			if(fabs(Aim_Real_Xcoord-Aim_Set_Xcoord)<0.5&&fabs(Aim_Real_Ycoord-Aim_Set_Ycoord)<0.5&&aim_sem_releas_flag==1)
			{
				aim_sem_releas_flag=0;
				rt_sem_release(aimtarget_sem);
			}
			break;
		}
		case Forward_Switch:
		{
			Car_SpeedX=110;
			break;
		}
		case Backward_Switch:
		{
			Car_SpeedX=-110;
			break;
		}
		default:
			break;
	}

	Car_SpeedZ=-PositPID(&A_P_PID,yaw*100,Car_AngleZ*100);
	if(abs(Car_SpeedZ)<20&&angle_sem_releas_flag==1)
	{
		angle_sem_releas_flag=0;
		rt_sem_release(mova_sem);
	}


	Car_SpeedX=LimitProtect(Car_MaxSpeedX,-Car_MaxSpeedX,Car_SpeedX);
	Car_SpeedY=LimitProtect(Car_MaxSpeedY,-Car_MaxSpeedY,Car_SpeedY);
	Car_SpeedZ=LimitProtect(Car_MaxSpeedZ,-Car_MaxSpeedZ,Car_SpeedZ);
	

	LGH_Motor1.Fit_Speed=Car_SpeedX+Car_SpeedY+Car_SpeedZ;//+LGH_Motor1.Ex_Speed;
	LGH_Motor2.Fit_Speed=Car_SpeedX-Car_SpeedY-Car_SpeedZ;//+LGH_Motor1.Ex_Speed;
	LGH_Motor3.Fit_Speed=Car_SpeedX+Car_SpeedY-Car_SpeedZ;//+LGH_Motor1.Ex_Speed;
	LGH_Motor4.Fit_Speed=Car_SpeedX-Car_SpeedY+Car_SpeedZ;//+LGH_Motor1.Ex_Speed;
	

	LGH_Motor1.Dutycycle +=PositPID(&M1_S_PID,LGH_Motor1.Encoder_Delta,LGH_Motor1.Fit_Speed);
	LGH_Motor2.Dutycycle +=PositPID(&M2_S_PID,LGH_Motor2.Encoder_Delta,LGH_Motor2.Fit_Speed);
	LGH_Motor3.Dutycycle +=PositPID(&M3_S_PID,LGH_Motor3.Encoder_Delta,LGH_Motor3.Fit_Speed);
	LGH_Motor4.Dutycycle +=PositPID(&M4_S_PID,LGH_Motor4.Encoder_Delta,LGH_Motor4.Fit_Speed);
	


	LGH_Motor1.Dutycycle=LimitProtect(Car_MaxDuty,-Car_MaxDuty,LGH_Motor1.Dutycycle);
	LGH_Motor2.Dutycycle=LimitProtect(Car_MaxDuty,-Car_MaxDuty,LGH_Motor2.Dutycycle);
	LGH_Motor3.Dutycycle=LimitProtect(Car_MaxDuty,-Car_MaxDuty,LGH_Motor3.Dutycycle);
	LGH_Motor4.Dutycycle=LimitProtect(Car_MaxDuty,-Car_MaxDuty,LGH_Motor4.Dutycycle);
	
	motorDrv1.setPWM(LGH_Motor1.Dutycycle);
	motorDrv2.setPWM(LGH_Motor2.Dutycycle);
	motorDrv3.setPWM(LGH_Motor3.Dutycycle);
	motorDrv4.setPWM(LGH_Motor4.Dutycycle);
  
}
